/*
 *  RigidBody3d4p.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 21/03/11
 *	Modified by Roberto Ortega 01/04/2011
 */

#include "rigidbody3d4p.h"

RigidBody3d4p::RigidBody3d4p(ifstream &in, string cname, MecSystem *pSys)
:	Body(in,cname,pSys)
{	
	dim = parentSystem->dim;	/** Must be set always.  dim belongs to the mother class Body */
	type = "rigidbody3d4p";
	
	contactAllOption = false;
	contactFileOption = false;
	contactNamesOption = false;
	
	int i;
	int j;

	char keyword[20];
	string auxs;				/** Aux string */
	vector<string> auxsv(4);
	vector<vector<double> > matRel;	// row: coordinates of a relative vector. 3 rows, 3 columns each
	cm.resize(dim);			// Principal moments of inertia
	vector<double> pmInertia(dim);			// Principal moments of inertia
	double mass;
	vector<vector<double> > Ri(dim);		// Initial rotation matrix
	for (i=0; i<dim; ++i)	Ri[i].resize(dim);
	vector<double> coords(dim);

	matRel.resize(3);
	for (i = 0; i < matRel.size(); ++i)
		matRel[i].resize(3);
	
	R.resize(3);
	for (i = 0; i < R.size(); ++i)	R[i].resize(3);
	
	while (in >> keyword && strcmp(keyword,"ENDBODY") )
	{
		/* keyword has POINTS :  Read 4 points */
		// First point is assumed to be G (center of gravity)
		// Local system assumed to be principal
		 
		// Keyword has the MASS label
		in >> mass;
		
		// Reads the coordinates of the center of mass (cm)
		in >> auxs;		// CM label
		in >> cm[0];
		if (dim>=2)	in >> cm[1];
		if (dim==3)	in >> cm[2];
		
		// Reads the orientation of the principal axes
		in >> auxs;	// ORIENTATION label
		for (i=0; i<dim; ++i)
		{
			for (j=0; j<dim; ++j)
			{
				in >> Ri[i][j];
			}
		}
		
		// Reads the principal moments of inertia
		in >> auxs;	// INERTIA label
		for (i = 0; i < dim; ++i)
			in >> pmInertia[i];
		
		// Read the name of the graphical object
		in >> auxs;	// SURFACE 
		in >> surfName;
		
		// Read the coordinates of the local points
		// all means that the local points are all points of the visualization mesh
		// filename.dat reads the coordinates from the file
		in >> auxs;
		if (!auxs.compare("CONTACTPOINTS"))
		{
			in >> auxs;
			if (!auxs.compare("all"))	contactAllOption = true;
			if (!auxs.compare("file"))
			{
				contactFileOption = true;
			}
			if (!auxs.compare("names"))
			{
				contactNamesOption = true;
			}
		}
		else if(!auxs.compare("ENDBODY")) break;
	}
	
	// If nothing is said about contact, take by default the "all" option
	if (contactAllOption==false && contactFileOption==false && contactNamesOption==false)	contactAllOption = true;
	
	// Create the four points that define an orthonormal system
	// The names are bodyname-0, -1, -2, -3
	
	// r = rg + Ri*r0
	// First point (center of mass)
	auxsv[0] = name + "-0";	
	coords[0] = 0.0;
	coords[1] = 0.0;
	coords[2] = 0.0;
	rigidTransOver(coords, cm, Ri);
	parentSystem->addPointName(auxsv[0],coords);
	
	// Second point
	auxsv[1] = name + "-1";	
	coords[0] = 1.0;
	coords[1] = 0.0;
	coords[2] = 0.0;
	rigidTransOver(coords, cm, Ri);
	parentSystem->addPointName(auxsv[1],coords);
	
	// Third point
	auxsv[2] = name + "-2";	
	coords[0] = 0.0;
	coords[1] = 1.0;
	coords[2] = 0.0;
	rigidTransOver(coords, cm, Ri);
	parentSystem->addPointName(auxsv[2],coords);
	
	// Third point
	auxsv[3] = name + "-3";	
	coords[0] = 0.0;
	coords[1] = 0.0;
	coords[2] = 1.0;
	rigidTransOver(coords, cm, Ri);
	parentSystem->addPointName(auxsv[3],coords);
	
	// matRel definition
	matRel[0][0] = 1.0;
	matRel[0][1] = 0.0;
	matRel[0][2] = 0.0;
	
	matRel[1][0] = 0.0;
	matRel[1][1] = 1.0;
	matRel[1][2] = 0.0;
	
	matRel[2][0] = 0.0;
	matRel[2][1] = 0.0;
	matRel[2][2] = 1.0;
	
	// Create Component: Rb3d4p
	vComponents.push_back(new Rb3d4p(mass,
									 auxsv,
									 matRel,
									 pmInertia,
									 dim));
	
	if(parentSystem->getProcNum()==parentSystem->getRankNum())
	cout << "	Body " << name << " succesfully read \n";
}

// ***

void RigidBody3d4p::setPTPointsComp()
{
	int j, k;
	vector<string> auxs;
	map<string, Point*, less<string> > maux;
	pair<string,Point*> mauxEntry;
	pair<string,int> sharedEntry;
	
	for (j = 0; j < vComponents.size(); j++)		
	//Loop on the components
	{
		auxs = (vComponents[j]->getPointNames());
		for(k = 0; k < auxs.size(); k++)		
		// Loop on the names of the points within each component
		{
			/** If the point it is not at the local process ... */
			if( (parentSystem->checkIfPointProcess(auxs[k])) == false )
			{
//				 cout << "Punto " << auxs[k] << " es remoto, estoy en el sistema " << parentSystem->name << endl;
				/** ...  create it at root system, copying the original point from the remote process */
				parentSystem->createPointAtRoot( parentSystem->getPTPoint(auxs[k]) );
				/** ...  and add the name of the shared point at the remote process */
				parentSystem->addRemoteSharedPoint(auxs[k],parentSystem->getProcNum());
			}
			mauxEntry.first = auxs[k];
			mauxEntry.second = (parentSystem->getPTPointLocal(auxs[k]));
			maux.insert(mauxEntry);
		}
		vComponents[j]->setMPoints(maux);
		maux.clear();
	}
}

// ***

void RigidBody3d4p::setPTPointsLoad()
{
	int j, k;
	vector<string> auxs;
	map<string, Point*, less<string> > maux;
	pair<string,Point*> mauxEntry;
	pair<string,int> sharedEntry;
	
	for(j=0; j<vLoads.size(); j++)		
	//Loop on the Loads
	{
		auxs = (vLoads[j]->getPointNames());
		for(k=0; k<auxs.size(); k++)		
		// Loop on the names of the points within each component
		{
			/** If the point it is not at the local process ... */
			if( (parentSystem->checkIfPointProcess(auxs[k]))==false )
			{
				/** ...  create it at root system, copying the original point from the remote process */
				parentSystem->createPointAtRoot( parentSystem->getPTPoint(auxs[k]) );
				/** ...  and add the name of the shared point at the remote process */
				parentSystem->addRemoteSharedPoint(auxs[k],parentSystem->getProcNum());
			}
			mauxEntry.first = auxs[k];
			mauxEntry.second = (parentSystem->getPTPointLocal(auxs[k]));
			maux.insert(mauxEntry);
		}
		vLoads[j]->setMPoints(maux);
		maux.clear();
	}
}

// ***

void RigidBody3d4p::setPPointSurface()
{}

// ***
void RigidBody3d4p::setMatAtComp()
{}

// ***
void RigidBody3d4p::getBoundary(vector<vector<string> >& connect)
{}

// ***

void RigidBody3d4p::writeMSH(ofstream &fGraph)
{}

// ***

void RigidBody3d4p::writeVTU(ofstream &fGraph, bool& plotDispFlag, bool& plotProcFlag, bool& plotStressFlag)
{
	if (cm.size()==0)	cm.resize(3);
	
	cm[0] = (vComponents[0]->q)[0];
	cm[1] = (vComponents[0]->q)[1];
	cm[2] = (vComponents[0]->q)[2];
	
	calcR();	
//	updateLocalPoints();
//	surface->writeVTU(vLocalPoints,localFGraph );
	
	surface->writeVTU(cm,R,localFGraph);
}

// ***

void RigidBody3d4p::writeOOGL(ofstream &fGraph)
{}

// ***
/*
void RigidBody3d4p::updateLocalPoints(void)
{
	vector<double> q = vComponents[0]->q;
	cm[0] = q[0];
	cm[1] = q[1];
	cm[2] = q[2];
	
	calcR();
	
//	surface->updateLocalPoints(q,vLocalPoints);
//	surface->updateLocalPoints(cm,R,vLocalPoints);
}
*/
// ***

void RigidBody3d4p::calcR(void)
{
	vector<double> q = vComponents[0]->q;
	
	R[0][0] = q[3]-q[0];
	R[1][0] = q[4]-q[1];
	R[2][0] = q[5]-q[2];
	
	R[0][1] = q[6]-q[0];
	R[1][1] = q[7]-q[1];
	R[2][1] = q[8]-q[2];
	
	R[0][2] = q[9]-q[0];
	R[1][2] = q[10]-q[1];
	R[2][2] = q[11]-q[2];
	
}

// ***

void RigidBody3d4p::writeFileTemp(const double& time, ofstream & outF)
{}

// ***

void RigidBody3d4p::setIniPosBody(vector<double>& iniValues)
{}
